//***********PID Constants**********

//Proportional and Integral value for Row and Pitch:
#define Kp 200
#define Ki 0.5
//0.4
//Kp 20 Ki 1
//And for Yaw:
#define yKp 0
#define yKi 0
//Maximum Integral value
#define pidMAX 200

//Motor controllers
int value[6] = {
  0,0,0,0,0,0};
//Difference between input variable and current state
float diffRow = 0;
float diffPit = 0;
float diffYaw = 0;
//Integral of difference
float intRow = 0;
float intPit = 0;
float intYaw = 0;
//Final PID output
float pidRow = 0;
float pidPit = 0;
float pidYaw = 0;

void motorCommand(void){
  //Emergency stop

  if(eStop == true){
    for(int i = 0; i <= 5; i++){
      analogWrite(pwmPin[i], 0); // Set motors off
    }
    intRow = 0.0;//Reset integral values
    intPit = 0.0;
    intYaw = 0.0;
  } 
  else {
    //********Input signal to motors********
    for(int i = 0; i <= 5; i++){
      if(value[i] > 255){
        value[i] = 255;
      }
      else if (value[i] < 20) {
        value[i] = 0;
      }
      analogWrite(pwmPin[i], value[i]); // aceita de 0 a 255
    }
  }  
}

void manualControl (void){
  //****** Input Processing********
  //Up
  for(int i=0;i<=5;i++){
    value[i] = (joyZ);
  } 
  //Row
  diffRow = -inpRow -imuRow ;
  //- inpRow;
  intRow += diffRow;
  pidRow = Kp*diffRow + Ki*intRow;
  pidRow = constrain(pidRow, -pidMAX,pidMAX);

  value[1] -= pidRow;
  value[2] -= pidRow;
  value[4] += pidRow;
  value[5] += pidRow;

  //Pitch
  diffPit = -inpPit -imuPit;
  //- inpPit;
  intPit += diffPit;
  pidPit = Kp*diffPit + Ki*intPit;
  pidPit = constrain(pidPit, -pidMAX,pidMAX);

  value[0] -= pidPit;
  value[3] += pidPit;

  //Yaw
  diffYaw = -inpYaw; //- imuYaw;
  intYaw += diffYaw;
  pidYaw = yKp*diffYaw + yKi*intYaw;
  pidYaw = constrain(pidYaw, -pidMAX,pidMAX);

  value[1] += pidYaw;
  value[2] -= pidYaw;
  value[4] += pidYaw;
  value[5] -= pidYaw;

  //Drive motors
  motorCommand();

}

void autoControl (void){
  //****** Input Processing********
  //Up
  for(int i=0;i<=5;i++){
    value[i] = joyZ;
  } 

  //Row
  diffRow = 0 - imuRow;
  intRow += diffRow;
  pidRow = Kp*diffRow + Ki*intRow;
  pidRow = constrain(pidRow, -pidMAX,pidMAX);

  value[1] -= pidRow;
  value[2] -= pidRow;
  value[4] += pidRow;
  value[5] += pidRow;

  //Pitch
  diffPit = 0 -imuPit;
  intPit += diffPit;
  pidPit = Kp*diffPit + Ki*intPit;
  pidPit = constrain(pidPit, -pidMAX,pidMAX);

  value[0] -= pidPit;
  value[3] += pidPit;

  //Yaw

  diffYaw = 0 - imuYaw;
  intYaw += diffYaw;
  pidYaw = yKp*diffYaw + yKi*intYaw;
  pidYaw = constrain(pidYaw, -pidMAX,pidMAX);

  value[1] += pidYaw;
  value[2] -= pidYaw;
  value[4] += pidYaw;
  value[5] -= pidYaw;

  //Drive motors
  motorCommand();
}




